// ===========================================================================
// KinectDevice.cpp : Source file for KinectDevice class
// ---------------------------------------------------------------------------
// Copyright (C) 2011 by Kevin Fisher
// ---------------------------------------------------------------------------
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//-----------------------------------------------------------------------------
// Author: Kevin Fisher <fisher.kevin.g -at- gmail.com> 
// Date:   Sat Aug 13 2011 15:38 EST
// ================================================================================================================================ 

#include "KinectDevice.h"

// ================================================================================================================================ 

const int KinectDevice::RGB_WIDTH              = 640;
const int KinectDevice::RGB_HEIGHT             = 480;
const int KinectDevice::DEPTH_WIDTH            = 640;
const int KinectDevice::DEPTH_HEIGHT           = 480;
const int KinectDevice::BUFFER_SIZE            = 640 * 480 * 4;

// ================================================================================================================================ 

/// <summary>
/// Constructor - Initializes the Kinect Device
/// </summary>
/// <param name="ctx">?</param>
/// <param name="index">The index of the Kinect device. Used for multiple connected devices.</param>
KinectDevice::KinectDevice(freenect_context *ctx, int index)
	: Freenect::FreenectDevice(ctx, index)
	, buffer_video(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes)
	, buffer_depth(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes)
	, gamma(2048)
	, new_rgb_frame(false)
	, new_depth_frame(false)
{
	// Initialize the gamma array
	for (unsigned int i = 0 ; i < 2048 ; i++)
	{
		float v = i / 2048.0;
		v = std::pow(v, 3) * 6;
		gamma[i] = v * 6 * 256;
	}
}

// ================================================================================================================================

/// <summary>
/// Destructor - Does nothing. 
/// </summary>
KinectDevice::~KinectDevice(void)
{
}

// ================================================================================================================================ 

/// <summary>
/// Retrieves the latest video buffer frame from the Kinect. 
/// </summary>
/// <param name="buffer">The array in which the video buffer frame will be stored.</param>
/// <returns>
/// False if there was not a new frame waiting. True if new buffer frame was found.
/// </returns>
bool KinectDevice::getRGB(uint8_t buffer[])
{
	Mutex::ScopeLock lock(this->rgb_mutex);
	if (!this->new_rgb_frame)
	{
		return false;
	}

	std::copy(this->buffer_video.begin(), this->buffer_video.end(), buffer);
	this->new_rgb_frame = false;
	return true;
}

// ================================================================================================================================ 

/// <summary>
/// Retrieves latest depth buffer frame from the Kinect
/// </summary>
/// <param name="buffer">The array in which the new depth buffer frame will be stored.</param>
/// <returns>
/// False if there was not a new frame waiting. True if new buffer frame was found.
/// </returns>
bool KinectDevice::getDepth(uint8_t buffer[])
{
	Mutex::ScopeLock lock(this->depth_mutex);
	if (!this->new_depth_frame)
	{
		return false;
	}

	std::copy(this->buffer_depth.begin(), this->buffer_depth.end(), buffer);
	this->new_depth_frame = false;
	return true;
}


// ================================================================================================================================ 

/// <summary>
/// Called by the freenect driver. Used to store the last video frame received from the Kinect. 
/// </summary>
/// <remarks>
/// \warning Do not call directly, even in the child. This is where the driver operates.
/// </remarks>
/// <param name="p_rgb">The new frame from the Kinect</param>
/// <param name="timestamp">Timestamp when frame was received</param>
void KinectDevice::VideoCallback(void *p_rgb, uint32_t timestamp)
{
	Mutex::ScopeLock lock(this->rgb_mutex);
	uint8_t *rgb = static_cast<uint8_t*>(p_rgb);
	std::copy(rgb, rgb + Freenect::FreenectDevice::getVideoBufferSize(), this->buffer_video.begin());
	this->new_rgb_frame = true;
}

// ================================================================================================================================ 

/// <summary>
/// Called by the freenect driver. Used to store the last depth frame received from the Kinect. 
/// </summary>
/// <remarks>
/// \warning Do not call directly, even in the child. This is where the driver operates.
/// </remarks>
/// <param name="p_depth">The new depth frame from the Kinect</param>
/// <param name="timestamp">Timestamp when frame was received</param>
void KinectDevice::DepthCallback(void *p_depth, uint32_t timestamp)
{
	Mutex::ScopeLock lock(this->depth_mutex);
	uint16_t* depth = static_cast<uint16_t*>(p_depth);
	for( unsigned int i = 0 ; i < 640*480 ; i++) {
		int pval = this->gamma[depth[i]];
		int lb = pval & 0xff;
		switch (pval>>8) {
		case 0:
			this->buffer_depth[3*i+0] = 255;
			this->buffer_depth[3*i+1] = 255-lb;
			this->buffer_depth[3*i+2] = 255-lb;
			break;
		case 1:
			this->buffer_depth[3*i+0] = 255;
			this->buffer_depth[3*i+1] = lb;
			this->buffer_depth[3*i+2] = 0;
			break;
		case 2:
			this->buffer_depth[3*i+0] = 255-lb;
			this->buffer_depth[3*i+1] = 255;
			this->buffer_depth[3*i+2] = 0;
			break;
		case 3:
			this->buffer_depth[3*i+0] = 0;
			this->buffer_depth[3*i+1] = 255;
			this->buffer_depth[3*i+2] = lb;
			break;
		case 4:
			this->buffer_depth[3*i+0] = 0;
			this->buffer_depth[3*i+1] = 255-lb;
			this->buffer_depth[3*i+2] = 255;
			break;
		case 5:
			this->buffer_depth[3*i+0] = 0;
			this->buffer_depth[3*i+1] = 0;
			this->buffer_depth[3*i+2] = 255-lb;
			break;
		default:
			this->buffer_depth[3*i+0] = 0;
			this->buffer_depth[3*i+1] = 0;
			this->buffer_depth[3*i+2] = 0;
			break;
		}
	}
	this->new_depth_frame = true;
}

// ================================================================================================================================ 

// ------------------------------[ END OF CODE ]-------------------------------
//
/// \file KinectDevice.cpp
/// \author Kevin Fisher <fisher.kevin.g -at- gmail.com> 
/// \date \c Created: Sat Aug 13 2011 15:18 EST

